 /*=============================================================================

MOTION INTERFACE MODULE

FILE: HMot.cpp

DESCRIPTION

Motion interface base module definition

EXTERNALIZED FUNCTIONS


Copyright (c) DaeSung Engineering Inc. All rights reserved.

=============================================================================*/
/*=============================================================================

EDIT HISTORY

---------  -------  -------------------------------------------------------
when       who     what, where, why
---------  -------  -------------------------------------------------------
07/17/04   nso4th   Frame Grabber Interface

=============================================================================*/

/*=============================================================================
HEADER INCLUSION
=============================================================================*/
#include "HMot.h"
#include "HMot_cfg.h"
#include <Cust_Target.h>
#include <SYS_app.h>
//#include "motHal_UMAC_cust.h"

#include <string.h>
#include <stdio.h>

/*=============================================================================
CONSTANT DEFINITION
=============================================================================*/

/*=============================================================================
LOCALIZED FUNCTIONS
=============================================================================*/
static dword motEventCallback(dword dwEvent, dword wParam, dword lParam, mcxt pUser);

/*=============================================================================
HMot CLASS IMPLEMENTATION
=============================================================================*/
/*-----------------------------------------------------------------------------
CONSTRUCTOR & DESTRUCTOR
-----------------------------------------------------------------------------*/
HMot::HMot()
{
	Event_SetCallback(motEventCallback, this);

	m_axNum = 0;
	m_motState = HMOT_NONE_S;
	m_homeState = HMOT_HOME_NONE_S;
	m_tblVel = m_tblAccel = m_tblPos = NULL;

	m_tblSWn = m_tblSWp = NULL;
	m_tblSW = NULL;
	m_errCode = 0;//HFGBASE_NONE_EC;
	memZeros(m_pszErr, sizeof(m_pszErr));

	m_motFTbl = new MOTFTbl;

	m_wHomeMask = 0x00;
	m_wHomeReqMask = 0x00;
	m_wHomeDoneMask = 0x00;

}

HMot::~HMot()
{
	Close();

	if(m_tblVel)
	{
		memFree(m_tblVel);
		m_tblVel = NULL;
	}
	if(m_tblAccel)
	{
		memFree(m_tblAccel);
		m_tblAccel = NULL;
	}
	if(m_tblPos)
	{
		memFree(m_tblPos);
		m_tblPos = NULL;
	}
	if(m_motFTbl)
	{
		delete m_motFTbl;
		m_motFTbl = NULL;
	}
	if(m_tblSWn)
	{
		memFree(m_tblSWn);
		m_tblSWn = NULL;
	}
	if(m_tblSWp)
	{
		memFree(m_tblSWp);
		m_tblSWp = NULL;
	}
	if(m_tblSW)
	{
		memFree(m_tblSW);
		m_tblSW = NULL;
	}
}

/*-----------------------------------------------------------------------------
OPERATION
-----------------------------------------------------------------------------*/
boolean HMot::Init(char *pszcfg)
{
	return	INIT_LIB(pszcfg);
}

boolean HMot::Close(void)
{
	return	CLOSE_LIB();
}

dword HMot::GetState(void)
{
	return	m_motState;
}

dword HMot::SetState(dword newState)
{
	dword	oldState = m_motState;

	m_motState = newState;

	return	oldState;
}

/*=============================================================================
MOTIOIN CONTROL
=============================================================================*/
// Signal
boolean HMot::SigGet(int32 axID, dword sig, boolean *on)
{
	MOTFTbl		*mtbl;
	boolean		done, sig_on;

	if(!m_pDev)
		return	FALSE;

	if(!RANGE_IN(axID, 0, (m_axNum-1)))
		return	FALSE;

	mtbl = m_motFTbl;//(MOTFTbl*)MOT_CFGGet(m_pDev);
	if(mtbl == NULL)
		return	FALSE;

	done = mtbl->SigGet(axID, sig, &sig_on);

	if(done == FALSE)
		return	FALSE;

	*on = sig_on;

	return	TRUE;
}

boolean HMot::ServoOn(int32 axID, boolean bOn)
{
	if(!m_pDev)
		return	FALSE;

	if(!RANGE_IN(axID, 0, (m_axNum-1)))
		return	FALSE;

	if(MOT_SigSet(m_pDev, axID, MOT_SIG_SERVO, bOn) == FALSE)
	{
		return	FALSE;
	}
	if(bOn)
	{
		int32			j;

		// Wait 100 msec.
		for(j=0; j<2; j++)
		{
			Task_Delay(50);
			Task_Switch();
		}
	}

	return	TRUE;
}

boolean HMot::IsServoOn(int32 axID)
{
	boolean	bOn = FALSE;

	if(!m_pDev)
		return	FALSE;

	if(!RANGE_IN(axID, 0, (m_axNum-1)))
		return	FALSE;

	MOT_SigGet(m_pDev, axID, MOT_SIG_SERVO, &bOn);

	return	bOn;
}

boolean HMot::SetOrigin(int32 axID)
{	
	if(!m_pDev)
		return	FALSE;

	if(!RANGE_IN(axID, 0, (m_axNum-1)))
		return	FALSE;

	return	MOT_PosSet(m_pDev, axID, (0.0));
}

boolean HMot::MoveContinue(int32 axID, byte byDir)
{
	double64		dVel, dAccel;
	double64		vel, accel;

	if(!m_pDev)
		return FALSE;

	if(!RANGE_IN(axID, 0, (m_axNum-1)))
		return	FALSE;

	//if(IsHomeDone() == FALSE)
	//	return	FALSE;

	vel = VEL_CHECK(axID);
	accel = ACCEL_CHECK(axID);

	switch(byDir)
	{
	case MOT_DIR_POSITIVE:
		dVel = vel;
		dAccel = accel;
		break;

	case MOT_DIR_NEGATIVE:
		dVel = vel * (-1.0);
		dAccel = accel;
		break;

	default:
		return	FALSE;
	}

	return	MOT_MoveContinue(m_pDev, axID, dVel, dAccel);
}

boolean HMot::Move(int32 axID, double64 dPos)
{
	double64		vel, accel;
	double64		negPos, posPos;

	if(!m_pDev)
		return FALSE;

	if(!RANGE_IN(axID, 0, (m_axNum-1)))
		return	FALSE;

//	if(IsHomeDone() == FALSE)
//		return	FALSE;

	vel = VEL_CHECK(axID);
	accel = ACCEL_CHECK(axID);

	switch(axID)
	{
	case MOT_AXIS_X:
	case MOT_AXIS_Y:
	case MOT_AXIS_A:
	case MOT_AXIS_B:
	case MOT_AXIS_C:
	case MOT_AXIS_D:
	case MOT_AXIS_E:
	{
		if(IsSWenable(axID) == TRUE)
		{
			SWget(axID, &negPos, &posPos);
			if(!RANGE_IN(dPos, negPos, posPos))
			{
				return	FALSE;
			}
		}
		break;
	}
	default:
		return	FALSE;
	}

	if(MOT_Move(m_pDev, axID, dPos, vel, accel) == TRUE)
	{
		*(m_tblPos+axID) = dPos;
	}

	return	FALSE;
}

boolean HMot::MoveOverride(int32 axID, double64 dPos)
{
	double64				vel, accel;

	if(!m_pDev)
		return FALSE;

	if(!RANGE_IN(axID, 0, (m_axNum-1)))
		return	FALSE;

//	if(IsHomeDone() == FALSE)
//		return	FALSE;

	vel = VEL_CHECK(axID);
	accel = ACCEL_CHECK(axID);

	return	MOT_MoveOverride(m_pDev, axID, dPos, vel, accel);
}

boolean HMot::IsMoveDone(int32 axID)
{
	boolean				bOn = FALSE;

#ifdef TG_SYSMGR_SIMUL

	bOn = TRUE;

#else
	MOTFTbl			*mtbl;

	if(!m_pDev)
		return	FALSE;

	mtbl = m_motFTbl;//(MOTFTbl*)MOT_CFGGet(m_pDev);

	if(!RANGE_IN(axID, 0, (m_axNum-1)))
		return	FALSE;

	MOT_MoveDone(m_pDev, axID, &bOn);
#endif
	return	bOn;
}

boolean HMot::EStop(boolean on)
{
	int32		j;

	if(!m_pDev)
		return	FALSE;

	if(on)
	{
		for(j=0; j<m_axNum; j++)
		{
			MOT_HomeStop(m_pDev, j);
			MOT_PathStop(m_pDev);
			MOT_Stop(m_pDev, j);
			ServoOn(j, FALSE);
		}
	}

	return	TRUE;
}


boolean HMot::Stop(int32 axID)
{
	if(!m_pDev)
		return	FALSE;

	if(!RANGE_IN(axID, 0, (m_axNum-1)))
		return	FALSE;

	return	MOT_Stop(m_pDev, axID);
}

boolean HMot::Home(void)
{
	//int32		i;

	if(!m_pDev)
		return	FALSE;

	if(GetState() == HMOT_PATH_S)
		return	FALSE;

	SetHomeState(HMOT_HOME_NONE_S);

	m_wHomeReqMask = 0x00;
	//for(i=0; i<m_axNum; i++)
	{
		if(Home(MOT_AXIS_Y) == FALSE)
		{
			return	FALSE;
		}
	}

	return	TRUE;
}

boolean HMot::Home(int32 axID)
{
	if(!m_pDev)
		return	FALSE;

	if(GetState() == HMOT_PATH_S)
		return	FALSE;

	ClearAlarm(axID);
	if(IsServoOn(axID) == FALSE)
	{
		ServoOn(axID, TRUE);
	}
	m_wHomeReqMask |= ((0x01) << axID);
	m_wHomeMask &= (~m_wHomeReqMask);

	return	MOT_Home(m_pDev, axID);
}

boolean HMot::HomeStop(void)
{
	int32		i;
	boolean		do_home = FALSE;

	if(!m_pDev)
		return	FALSE;

	do_home = IsHomming();

	for(i=0; i<m_axNum; i++)
	{
		MOT_HomeStop(m_pDev, i);
	}
	if(do_home)
	{
		SetHomeState(HMOT_HOME_NONE_S);
		m_wHomeReqMask = 0x00;
	}

	return	TRUE;
}

boolean HMot::HomeStop(int32 axID)
{
	boolean		do_home = FALSE;

	if(!m_pDev)
		return	FALSE;

	do_home = IsHomming(axID);

	MOT_HomeStop(m_pDev, axID);

	if(do_home)
	{
		SetHomeState(HMOT_HOME_NONE_S);
		m_wHomeReqMask &= ~(0x01<<axID);
	}

	return	TRUE;
}

boolean HMot::IsHomeDone(void)
{
	if(!m_pDev)
		return	FALSE;

	if(m_homeState == HMOT_HOME_NONE_S)
		return	FALSE;

	if(m_wHomeMask != m_wHomeDoneMask)
	{
		return	FALSE;
	}

	return	TRUE;
}

boolean HMot::IsHomeDone(int32 axID)
{
	if(!m_pDev)
		return	FALSE;

	if(!RANGE_IN(axID, 0, (m_axNum-1)))
		return	FALSE;

	if(ISON(m_wHomeMask, axID) == TRUE)
		return	TRUE;

	return	FALSE;
}

boolean HMot::IsHomming(void)
{
	if(!m_pDev)
		return	FALSE;

	return	MOT_IsHomming(m_pDev);
}

boolean HMot::IsHomming(int32 axID)
{
	if(!m_pDev)
		return	FALSE;

	if(!RANGE_IN(axID, 0, (m_axNum-1)))
		return	FALSE;

	if(ISON(m_wHomeReqMask, axID) == TRUE)
		return	TRUE;

	return	FALSE;
}

boolean HMot::HomeClear(void)
{
	int32		j;

	if(!m_pDev)
		return	FALSE;

	for(j=0; j<m_axNum; j++)
	{
		if(HomeClear(j) == FALSE)
			return	FALSE;
	}
	return	TRUE;
}

boolean HMot::HomeClear(int32 axID)
{
	dword		mask;

	if(!m_pDev)
		return	FALSE;

	if(!RANGE_IN(axID, 0, (m_axNum-1)))
		return	FALSE;

	SWenable(axID, FALSE);
	
	if(IsMoveDone(axID) == FALSE)
		return	FALSE;

	if(IsHomming(axID) == TRUE)
		return	FALSE;

	mask = ((0x01) << axID);
	m_wHomeReqMask &= (~mask);
	m_wHomeMask &= (~mask);

	return	TRUE;
}


boolean HMot::Path(mcxt mPath)
{
	if(mPath == NULL)
		return	FALSE;

	if(GetState() != HMOT_NORMAL_S)
		return	FALSE;

	return	MOT_Path(m_pDev, mPath, 0);
}

boolean HMot::PathStop(void)
{
	if(!m_pDev)
		return	FALSE;

	return	MOT_PathStop(m_pDev);
}

boolean HMot::PathLock(void)
{
	if(!m_pDev)
		return	FALSE;

	return	MOT_PathLock(m_pDev);
}

boolean HMot::PathUnlock(void)
{
	if(!m_pDev)
		return	FALSE;

	return	MOT_PathUnlock(m_pDev);
}

boolean HMot::IsPathMoving(void)
{
	if(m_motFTbl == NULL)
		return	FALSE;

	return	m_motFTbl->IsPathMoving();
}

boolean HMot::UserOutput(int32 axID, int32 iChan, boolean bOn)
{
	if(!m_pDev)
		return	FALSE;

	if(!RANGE_IN(axID, 0, (m_axNum-1)))
		return	FALSE;

	return	MOT_UserOutput(m_pDev, axID, iChan, bOn);
}

boolean HMot::UserOutputGet(int32 axID, int32 iChan)
{
	boolean		bOn = FALSE;

	if(!m_pDev)
		return	FALSE;

	if(!RANGE_IN(axID, 0, (m_axNum-1)))
		return	FALSE;

	MOT_UserOutputGet(m_pDev, axID, iChan, &bOn);

	return	bOn;
}

boolean HMot::UserInput(int32 axID, int32 iChan)
{
	boolean		bOn = FALSE;

	if(!m_pDev)
		return	FALSE;

	if(!RANGE_IN(axID, 0, (m_axNum-1)))
		return	FALSE;

	MOT_UserInput(m_pDev, axID, iChan, &bOn);

	return	bOn;
}

boolean HMot::UserInput_On(boolean on)
{
	return	TRUE;
}


boolean HMot::IsInput(dword dwChan)
{
	if(m_motFTbl == NULL)
		return	FALSE;

	return	m_motFTbl->IsInput(dwChan);
}

boolean HMot::Input32(dword *pIn)
{
	if(m_motFTbl == NULL)
		return	FALSE;

	return	m_motFTbl->Input32(pIn);
}

boolean HMot::Input32Ex(dword modID, dword *pIn)
{
	if(m_motFTbl == NULL)
		return	FALSE;

	return	m_motFTbl->Input32Ex(modID, pIn);
}


/* Output */
boolean HMot::IsOutput(dword dwChan)
{
	if(m_motFTbl == NULL)
		return	FALSE;

	return	m_motFTbl->IsOutput(dwChan);
}

boolean HMot::IsOutput32(dword *pOut)
{
	if(m_motFTbl == NULL)
		return	FALSE;

	return	m_motFTbl->IsOutput32Ex(0, pOut);
}

boolean HMot::IsOutput32Ex(dword modID, dword *pOut)
{
	if(m_motFTbl == NULL)
		return	FALSE;
#if 1
	{
		int32			k, j, base;
		boolean			on;
		dword			sts = 0x00000000;

		base = modID*32;

		for(k=0, j=base; k<32; j++, k++)
		{
			on = m_motFTbl->IsOutput(j);

			if(on)
			{
				sts |= (0x01 << k);
			}
			else
			{
			}
		}
		*pOut = sts;

		return	TRUE;
	}
#else

	return	m_motFTbl->IsOutput32Ex(modID, pOut);
#endif
}

boolean HMot::Output(dword dwChan, boolean bOn)
{
	if(m_motFTbl == NULL)
		return	FALSE;

	return	m_motFTbl->Output(dwChan, bOn);
}

boolean HMot::Output32(dword dwOut)
{
	if(m_motFTbl == NULL)
		return	FALSE;

	return	m_motFTbl->Output32(dwOut);
}

boolean HMot::Output32Ex(dword modID, dword dwOut)
{
	if(m_motFTbl == NULL)
		return	FALSE;

	return	m_motFTbl->Output32Ex(modID, dwOut);
}

// TRIGGER
boolean HMot::TrigEnable(int32 axID, boolean bEnable)
{
	if(!m_pDev)
		return	FALSE;

	if(!RANGE_IN(axID, 0, (m_axNum-1)))
		return	FALSE;

	return	MOT_TrigEnable(m_pDev, axID, bEnable);
}

boolean HMot::TrigSetTable(int32 axID, double64 dStart, double64 dEnd, double64 dInterval)
{
	if(!m_pDev)
		return	FALSE;

	if(!RANGE_IN(axID, 0, (m_axNum-1)))
		return	FALSE;

	return	MOT_TrigSetTable(m_pDev, axID, dStart, dEnd, dInterval);
}

boolean HMot::TrigInvert(int32 axID, boolean bEnable)
{
	MOTFTbl		*mtbl;

	if(!m_pDev)
		return	FALSE;

	if(!RANGE_IN(axID, 0, (m_axNum-1)))
		return	FALSE;

	mtbl = m_motFTbl;//(MOTFTbl*)MOT_CFGGet(m_pDev);
	if(mtbl == NULL)
	{
		return	FALSE;
	}

	return	mtbl->TrigInvert(axID, bEnable);
}

boolean HMot::TrigStart(int32 axID)
{
	if(!m_pDev)
		return	FALSE;

	if(!RANGE_IN(axID, 0, (m_axNum-1)))
		return	FALSE;

	return	MOT_TrigStart(m_pDev, axID);
}

boolean HMot::TrigStop(int32 axID)
{
	if(!m_pDev)
		return	FALSE;

	if(!RANGE_IN(axID, 0, (m_axNum-1)))
		return	FALSE;

	return	MOT_TrigStop(m_pDev, axID);
}

// Encoder direction
boolean HMot::EncInvert(int32 axID, boolean bEnable)
{
	MOTFTbl		*mtbl;

	if(!m_pDev)
		return	FALSE;

	if(!RANGE_IN(axID, 0, (m_axNum-1)))
		return	FALSE;

	mtbl = m_motFTbl;//(MOTFTbl*)MOT_CFGGet(m_pDev);
	if(mtbl == NULL)
	{
		return	FALSE;
	}

	return	mtbl->EncInvert(axID, bEnable);
}

boolean HMot::IsAlarm(int32 axID)
{
	boolean	bOn = FALSE;

	if(!m_pDev)
		return	FALSE;

	MOT_SigGet(m_pDev, axID, MOT_SIG_ALARM, &bOn);

	if(bOn)
	{
		SetHomeState(HMOT_HOME_NONE_S);
	}
	return	bOn;
}

boolean HMot::ClearAlarm(int32 axID)
{
	if(!m_pDev)
		return	FALSE;

	if(MOT_SigSet(m_pDev, axID, MOT_SIG_ALARM_CLEAR, TRUE))
	{
		int32		j;

		// Wait 300 msec.
		for(j=0; j<6; j++)
		{
			Task_Delay(50);
			Task_Switch();
		}
		MOT_SigSet(m_pDev, axID, MOT_SIG_ALARM_CLEAR, FALSE);
		// Wait 300 msec.
		for(j=0; j<6; j++)
		{
			Task_Delay(50);
			Task_Switch();
		}
		IsAlarm(axID);
	}
	else
	{
		return	FALSE;
	}
	return	TRUE;
}

boolean HMot::IsEMO(dword emoPos)
{
	MOTFTbl		*mtbl;

	if(!m_pDev)
		return	FALSE;

	mtbl = m_motFTbl;
	if(mtbl == NULL)
	{
		return	FALSE;
	}
	return	mtbl->IsEMO(emoPos);
}

boolean HMot::LoadRateGet(int32 axID, int32 *iRate)
{
	MOTFTbl		*mtbl;

	if(!m_pDev)
		return	FALSE;

	if(!RANGE_IN(axID, 0, (m_axNum-1)))
		return	FALSE;

	mtbl = m_motFTbl;
	if(mtbl == NULL)
	{
		return	FALSE;
	}

	return	mtbl->LoadRateGet(axID, iRate);
}

// E-GEAR
boolean HMot::EgearSet(int32 axID, int32 axMaster, double64 ratio)
{
	MOTFTbl		*mtbl;

	if(!m_pDev)
		return	FALSE;

	if(!RANGE_IN(axID, 0, (m_axNum-1)))
		return	FALSE;

	if(!RANGE_IN(axMaster, 0, (m_axNum-1)))
		return	FALSE;

	mtbl = m_motFTbl;
	if(mtbl == NULL)
	{
		return	FALSE;
	}

	switch(axID)
	{
	case MOT_AXIS_X:
	{
		if(axMaster != MOT_AXIS_X)
		{
			return	FALSE;
		}
		break;
	}
	case MOT_AXIS_Y:
	{
		switch(axMaster)
		{
		case MOT_AXIS_Y:
			{
				break;
			}
		default:
			return	FALSE;
		}
		break;
	}
	default:
	{
		return	mtbl->EgearSet(axID, axID, ratio); // Master
	}
	}

	return	mtbl->EgearSet(axID, axMaster, ratio);
}

boolean HMot::EgearIsSet(int32 axID)
{
	MOTFTbl		*mtbl;

	if(!m_pDev)
		return	FALSE;

	if(!RANGE_IN(axID, 0, (m_axNum-1)))
		return	FALSE;

	mtbl = m_motFTbl;
	if(mtbl == NULL)
	{
		return	FALSE;
	}
	return	mtbl->EgearIsSet(axID);
}

boolean HMot::SWenable(int32 axID, boolean enable)
{
	if(!m_pDev)
		return	FALSE;

	if(!RANGE_IN(axID, 0, (m_axNum-1)))
		return	FALSE;

	switch(axID)
	{
	case MOT_AXIS_X:	*(m_tblSW+MOT_AXIS_X) = enable;	break;
	case MOT_AXIS_Y:	*(m_tblSW+MOT_AXIS_Y) = enable;	break;
	case MOT_AXIS_A:	*(m_tblSW+MOT_AXIS_A) = enable;	break;
	case MOT_AXIS_B:	*(m_tblSW+MOT_AXIS_B) = enable;	break;
	case MOT_AXIS_C:	*(m_tblSW+MOT_AXIS_C) = enable;	break;
	case MOT_AXIS_D:	*(m_tblSW+MOT_AXIS_D) = enable;	break;
	case MOT_AXIS_E:	*(m_tblSW+MOT_AXIS_E) = enable;	break;
	default:
	{
		*(m_tblSW+axID) = enable;
		break;
	}
	}

	return TRUE;
}

boolean HMot::IsSWenable(int32 axID)
{
	if(!m_pDev)
		return	FALSE;

	if(!RANGE_IN(axID, 0, (m_axNum-1)))
		return	FALSE;

	return *(m_tblSW+axID);
}

boolean HMot::SWset(int32 axID, double64 negPos, double64 posPos)
{
	MOTFTbl		*mtbl;
	
	if(!m_pDev)
		return	FALSE;

	if(!RANGE_IN(axID, 0, (m_axNum-1)))
		return	FALSE;

	if(negPos >= posPos)
		return FALSE;

	mtbl = m_motFTbl;//(MOTFTbl*)MOT_CFGGet(m_pDev);
	if(mtbl == NULL)
	{
		return	FALSE;
	}

	*(m_tblSWn+axID) = negPos;
	*(m_tblSWp+axID) = posPos;

	return mtbl->SWLimitSet(axID, negPos, posPos);
	//return TRUE;
}

boolean HMot::SWget(int32 axID, double64 *negPos, double64 *posPos)
{
	if(!m_pDev)
		return	FALSE;

	if(!RANGE_IN(axID, 0, (m_axNum-1)))
		return	FALSE;

	*negPos = *(m_tblSWn+axID);
	*posPos = *(m_tblSWp+axID);

	return TRUE;
}

boolean HMot::HOMEset(int32 axID, double64 Hoffset)
{
	MOTFTbl		*mtbl;

	if(!m_pDev)
		return	FALSE;

	if(!RANGE_IN(axID, 0, (m_axNum-1)))
		return	FALSE;

	mtbl = m_motFTbl;//(MOTFTbl*)MOT_CFGGet(m_pDev);
	if(mtbl == NULL)
	{
		return	FALSE;
	}

	return	mtbl->HOMEset(axID, Hoffset);
}

boolean HMot::HOMEget(int32 axID, double64 *Hoffset)
{
	MOTFTbl		*mtbl;

	if(!m_pDev)
		return	FALSE;

	if(!RANGE_IN(axID, 0, (m_axNum-1)))
		return	FALSE;

	mtbl = m_motFTbl;//(MOTFTbl*)MOT_CFGGet(m_pDev);
	if(mtbl == NULL)
	{
		return	FALSE;
	}

	return	mtbl->HOMEget(axID, Hoffset);
}

/*=============================================================================
PARAMETER & STATUS INTERFACE
=============================================================================*/
int32 HMot::GetAxisNum(void)
{
	return	m_axNum;
}

boolean HMot::SetVelocity(int32 axID, double64 dVel)
{
	if(!m_pDev)
		return	FALSE;

	if(!RANGE_IN(axID, 0, (m_axNum-1)))
		return	FALSE;

	if(dVel <= 0.0)
		return	FALSE;

	switch(axID)
	{
	case MOT_AXIS_X:	dVel = MIN(dVel, MOT_VEL_MAX_X);	break;
	case MOT_AXIS_Y:	dVel = MIN(dVel, MOT_VEL_MAX_Y);	break;
	case MOT_AXIS_A:	dVel = MIN(dVel, MOT_VEL_MAX_A);	break;
	case MOT_AXIS_B:	dVel = MIN(dVel, MOT_VEL_MAX_B);	break;
	case MOT_AXIS_C:	dVel = MIN(dVel, MOT_VEL_MAX_C);	break;
	case MOT_AXIS_D:	dVel = MIN(dVel, MOT_VEL_MAX_D);	break;
	case MOT_AXIS_E:	dVel = MIN(dVel, MOT_VEL_MAX_E);	break;
	}
	*(m_tblVel+axID) = dVel;

	return	SetAccel(axID, (dVel*2.0));
}

double64 HMot::GetVelocity(int32 axID)
{
	if(!m_pDev)
		return	FALSE;

	if(!RANGE_IN(axID, 0, (m_axNum-1)))
		return	0.0;

	return	*(m_tblVel+axID);
}

boolean HMot::SetAccel(int32 axID, double64 dAccel)
{
	if(!m_pDev)
		return	FALSE;

	if(!RANGE_IN(axID, 0, (m_axNum-1)))
		return	FALSE;

	*(m_tblAccel+axID) = dAccel;

	return	TRUE;
}

double64 HMot::GetAccel(int32 axID)
{
	if(!m_pDev)
		return	FALSE;

	if(!RANGE_IN(axID, 0, (m_axNum-1)))
		return	0.0;

	return	*(m_tblAccel+axID);
}

boolean HMot::SetHomeSpeed(int32 axID, double64 dSpeed)
{
	if(!m_pDev)
		return	FALSE;

	if(!RANGE_IN(axID, 0, (m_axNum-1)))
		return	FALSE;

	if(dSpeed < 0.0)
		return	FALSE;

	return	MOT_SetHomeSpeed(m_pDev, axID, dSpeed);
}

boolean HMot::GetPosition(int32 axID, double64 *pPos)
{
	boolean			done;
	double64			pos;

	if(!m_pDev)
		return	FALSE;

	if(!RANGE_IN(axID, 0, (m_axNum-1)))
		return	FALSE;

	done = MOT_PosGet(m_pDev, axID, &pos);
	if(done == FALSE)
		return	FALSE;

#ifdef TG_SYSMGR_SIMUL
	pos = *(m_tblPos+axID);
	*pPos = *(m_tblPos+axID);
#else
	*pPos = pos;
#endif

	return	TRUE;
}

boolean HMot::SetPosition(int32 axID, double64 dPos)
{
	if(!m_pDev)
		return	FALSE;

	if(!RANGE_IN(axID, 0, (m_axNum-1)))
		return	FALSE;

#ifdef TG_SYSMGR_SIMUL
	if(MOT_PosSet(m_pDev, axID, dPos) == TRUE)
	{
		*(m_tblPos + axID) = dPos;
		return	TRUE;
	}
	return	FALSE;
#else
	return	MOT_PosSet(m_pDev, axID, dPos);
#endif
}

boolean HMot::GetPositionCommanded(int32 axID, double64 *pPos)
{
	if(!m_pDev)
		return	FALSE;

	if(!RANGE_IN(axID, 0, (m_axNum-1)))
		return	FALSE;

#ifdef TG_SYSMGR_SIMUL
	*pPos = *(m_tblPos+axID);
	return	TRUE;
#else
	return	MOT_PosCommandGet(m_pDev, axID, pPos);
#endif
}

boolean HMot::GetVelocityActual(int32 axID, double64 *dVel)
{
	if(!m_pDev)
		return	FALSE;

	if(!RANGE_IN(axID, 0, (m_axNum-1)))
		return	FALSE;

	return	MOT_VelGet(m_pDev, axID, dVel);
}

mcxt HMot::Get_MotCxt(void)
{
	return	m_pDev;
}

/*---------------------------------------------------------
APPLICATION SPECIFIC.
---------------------------------------------------------*/

/*=============================================================================
PRIVATE MEMBERS
=============================================================================*/
void HMot::SetHomeState(dword homeState)
{
	dword	oldState = GetState();

	switch(homeState)
	{
	case HMOT_HOME_NONE_S:
		{
			m_wHomeMask = 0x00;

			switch(oldState)
			{
			case HMOT_NORMAL_S: SetState(HMOT_INIT_S); break;
			case HMOT_PATH_S: SetState(HMOT_INIT_S); break;
			default:
				SetState(HMOT_NONE_S);
				break;
			}
			break;
		}
	case HMOT_HOME_TRANSIT_S:
		{
			m_wHomeMask = 0x00;
			break;
		}
	case HMOT_HOME_DONE_S:
		{
			SetState(HMOT_NORMAL_S);
			break;
		}
	}
	m_homeState = homeState;
}

void HMot::SetAxisNum(int32 axNum)
{
	m_axNum = axNum;
}

double64 HMot::VEL_CHECK(int32 axID)
{
	double64			vel, vel_req;

	vel_req = *(m_tblVel+axID);
	vel = vel_req;

	if(vel_req < 0.0)
		return	(0.0);

	switch(axID)
	{
	case MOT_AXIS_X:
	{
		if(vel_req > MOT_VEL_MAX_X)
		{
			vel = MOT_VEL_MAX_X;
		}
		break;
	}
	case MOT_AXIS_Y:	
	{
		if(vel_req > MOT_VEL_MAX_Y)
		{
			vel = MOT_VEL_MAX_Y;
		}
		break;
	}
	case MOT_AXIS_A:
	{
		if(vel_req > MOT_VEL_MAX_A)
		{
			vel = MOT_VEL_MAX_A;
		}
		break;
	}
	case MOT_AXIS_B:	
	{
		if(vel_req > MOT_VEL_MAX_B)
		{
			vel = MOT_VEL_MAX_B;
		}
		break;
	}
	case MOT_AXIS_C:
	{
		if(vel_req > MOT_VEL_MAX_C)
		{
			vel = MOT_VEL_MAX_C;
		}
		break;
	}
	case MOT_AXIS_D:	
	{
		if(vel_req > MOT_VEL_MAX_D)
		{
			vel = MOT_VEL_MAX_D;
		}
		break;
	}
	case MOT_AXIS_E:
	{
		if(vel_req > MOT_VEL_MAX_E)
		{
			vel = MOT_VEL_MAX_E;
		}
		break;
	}
	default:
	{
		vel = 0.0;
		break;
	}
	}
	//DBGPRINT("axID = %d, vel = %.1f\n", axID, vel);

	return	vel;
}

double64 HMot::ACCEL_CHECK(int32 axID)
{
	double64			accel, accel_req;

	accel_req = *(m_tblAccel+axID);
	accel = accel_req;

	if(accel_req < 0.0)
		return	(0.0);

	return	accel;
}


/*=============================================================================
ERROR HANDLING
=============================================================================*/
// Error Access
dword HMot::Get_LastError(void)
{
	return	(0);
}

char *HMot::Get_ErrorString(dword err_code)
{
	return	&m_pszErr[0];
}

boolean HMot::INIT_LIB(char *pszname)
{
	ObjClient_s		c;
	int32			j, i, axNum;

	SetState(HMOT_NONE_S);

	// Context init.
	if(m_pDev != NULL)
	{
		return	FALSE;
	}

	m_pDevList[0] = MOT_Alloc(m_motFTbl);
	SetCurrentDevice(0);

	if(m_pDev == NULL)
	{
		MOT_Free(m_pDev);

		m_pDev = m_pDevList[0] = NULL;
		return	FALSE;
	}

	// Load the Configuration
	if(pszname == NULL)
	{
		MOT_Free(m_pDev);

		m_pDev = m_pDevList[0] = NULL;
		return	FALSE;
	}

	// Initialize
	SetHomeState(HMOT_HOME_TRANSIT_S);

	m_CliCxt[0].pThis = this;
	m_CliCxt[0].dwDevID = 0;

	c.pUserCxt = &m_CliCxt[0];
	c.EventCbf = m_pEvtCb;

	MOT_SetClient(m_pDev, &c);
	MOT_SetCfgFile(m_pDev, pszname);

	if(MOT_Init(m_pDev) == FALSE)
	{
		MOT_Free(m_pDevList[0]);
		m_pDev = m_pDevList[0] = NULL;

		return	FALSE;
	}

	// Vel. & Acc. Table
	axNum = MOT_AxisGetNum(m_pDev);
	SetAxisNum(axNum);

	m_tblVel = (double64*)memCalloc(sizeof(*m_tblVel)*axNum);
	m_tblAccel = (double64*)memCalloc(sizeof(*m_tblAccel)*axNum);
	m_tblPos = (double64*)memCalloc(sizeof(*m_tblPos)*axNum);
	m_tblSWn = (double64*)memCalloc(sizeof(*m_tblSWn)*axNum);
	m_tblSWp = (double64*)memCalloc(sizeof(*m_tblSWp)*axNum);	
	m_tblSW = (boolean*)memCalloc(sizeof(*m_tblSW)*axNum);	

	SetState(HMOT_INIT_S);
	m_wHomeDoneMask = 0x00;
	for(i=0; i<axNum; i++)
	{
		Stop(i);

		// Servo-On
		for(j=0; j<m_axNum; j++)
		{
			ServoOn(j, TRUE);
		}

		// E-STOP LEVEL

		// Alarm releasted
		if(IsAlarm(i) == TRUE)
		{
			ServoOn(i, FALSE);
			ClearAlarm(i);
		}

		// SW-Limit
		SWenable(i, TRUE);
		switch(i)
		{
		case MOT_AXIS_X:	SWset(i, MOT_POS_MIN_X, MOT_POS_MAX_X);	break;
		case MOT_AXIS_Y:	SWset(i, MOT_POS_MIN_Y, MOT_POS_MAX_Y);	break;
		case MOT_AXIS_A:	SWset(i, MOT_POS_MIN_A, MOT_POS_MAX_A);	break;
		case MOT_AXIS_B:	SWset(i, MOT_POS_MIN_B, MOT_POS_MAX_B);	break;
		case MOT_AXIS_C:	SWset(i, MOT_POS_MIN_C, MOT_POS_MAX_C);	break;
		case MOT_AXIS_D:	SWset(i, MOT_POS_MIN_D, MOT_POS_MAX_D);	break;
		case MOT_AXIS_E:	SWset(i, MOT_POS_MIN_E, MOT_POS_MAX_E);	break;
		}

		// Make Home done mask
		//if(i == MOT_AXIS_E)
		{
			m_wHomeDoneMask |= (0x01 << i);
		}
	}	
	
#ifdef TG_SYSMGR_SIMUL
	for(i=0; i<axNum; i++)
	{
		SWenable(i, TRUE);
	}
	m_wHomeMask = m_wHomeDoneMask;
	SetHomeState(HMOT_HOME_DONE_S);
#else	
	for(i=0; i<axNum; i++)
	{
		SWenable(i, TRUE);
	}
	m_wHomeMask = m_wHomeDoneMask;
	SetHomeState(HMOT_HOME_DONE_S);
	//SetHomeState(HMOT_HOME_NONE_S);
#endif

	return	TRUE;
}

boolean HMot::CLOSE_LIB(void)
{
	//int32		j;

	SetState(HMOT_NONE_S);

	if(m_pDevList[0] == NULL)
		return	TRUE;

	SetHomeState(HMOT_HOME_NONE_S);

	 //Servo-Off
	/*for(j=0; j<m_axNum; j++)
	{
		ServoOn(j, FALSE);
	}*/
	
	MOT_Close(m_pDev);
	MOT_Free(m_pDev);

	if(m_tblVel)
	{
		memFree(m_tblVel);
		m_tblVel = NULL;
	}
	if(m_tblAccel)
	{
		memFree(m_tblAccel);
		m_tblAccel = NULL;
	}
	if(m_tblPos)
	{
		memFree(m_tblPos);
		m_tblPos = NULL;
	}
	if(m_tblSWn)
	{
		memFree(m_tblSWn);
		m_tblSWn = NULL;
	}
	if(m_tblSWp)
	{
		memFree(m_tblSWp);
		m_tblSWp = NULL;
	}
	if(m_tblSW)
	{
		memFree(m_tblSW);
		m_tblSW = NULL;
	}

	memZeros(m_pDevList, sizeof(m_pDevList));
	m_pDev = NULL;

	return	TRUE;
}

void HMot::ERR_RESET(void)
{

}

void HMot::ERR_SET(dword err_code)
{
}

/*-------------------------------------------------------------------
EVENT HANDLER
-------------------------------------------------------------------*/
dword HMot::Event_Handler(dword dwEvent, dword wParam, dword lParam)
{
	if(Event_PreProc(dwEvent, wParam, lParam))
	{
		return	TRUE;
	}

	return	Event_Pass(dwEvent, wParam, lParam);
}

dword HMot::Event_PreProc(dword dwEvent, dword wParam, dword lParam)
{
	dword		event, evtID;
	int32		axID;

	event = HDEV_GETEVENT(dwEvent);
	evtID = HDEV_GETDEV_ID(dwEvent);

	switch(event)
	{
	case MOT_HOME_START_E:
	{
		return	FALSE;
	}
	case MOT_HOME_DONE_E:
	{
		word		wMask;

		axID = (int32)wParam;
		wMask = (0x01) << axID;

		m_wHomeMask = m_wHomeMask | wMask;

		// Update req mask
		m_wHomeReqMask &= (~wMask);

		SWenable(axID, TRUE);
		//DBGPRINT(" wMask = 0x%X, m_wHomeMask = 0x%X, m_wHomeReqMask = 0x%X\n", wMask, m_wHomeMask, m_wHomeReqMask); 
		DBGPRINT("[HMOT] %d Axis Homing was done !\n", wParam);

		// Compare the mask
		if(m_wHomeDoneMask== m_wHomeMask)
		{
			SetHomeState(HMOT_HOME_DONE_S);
			DBGPRINT("[HMOT] HOME FINISHED !!");
		}


		// Initial software limit

		break;
	}
	case MOT_HOME_ERR_E:
	{
		axID = (int32)wParam;

		SetHomeState(HMOT_HOME_NONE_S);
		return	FALSE;
	}
	case MOT_PATH_START_E:
	{
		SetState(HMOT_PATH_S);
		break;
	}
	case MOT_PATH_DONE_E:
	case MOT_PATH_ERR_E:
	{
		SetState(HMOT_NORMAL_S);
		break;
	}
	}
	return	FALSE;
}

dword motEventCallback(dword dwEvent, dword wParam, dword lParam, mcxt pUser)
{
	HDevCliCxt	*pCxt = (HDevCliCxt*)pUser;

	if(pCxt && pCxt->pThis)
	{
		return pCxt->pThis->Event_Handler(HDEV_SETDEV_ID(dwEvent, pCxt->dwDevID), wParam, lParam);
	}

	return	(0);
}

/*--- END OF HMot.cpp ---*/
